// ------------------------- 头文件包含 (Header Includes) -------------------------

#include "ros/ros.h"                                     // 包含ROS核心库头文件，提供节点、话题、服务等ROS基础功能。
#include <move_base_msgs/MoveBaseAction.h>                 // 包含move_base Action的消息类型定义，用于与导航功能栈交互。
#include <actionlib/client/simple_action_client.h>       // 包含actionlib的简单客户端库，方便创建和管理Action客户端。
#include <geometry_msgs/Twist.h>                         // 包含速度控制消息类型，通常用于发布速度指令(虽然本代码中未直接发布)。
#include <robot_audio/robot_iat.h>                         // 包含自定义的语音听写(iat)服务类型头文件。
#include <robot_audio/Collect.h>                           // 包含自定义的语音采集(Collect)服务类型头文件。
#include <robot_audio/robot_tts.h>                         // 包含自定义的语音合成(tts)服务类型头文件。

#include <iostream>                                      // 包含标准输入输出流库，用于控制台打印(如cout)。
#include <string>                                        // 包含标准字符串库，提供std::string类。

// ------------------------- 命名空间与宏定义 (Namespace & Macros) -------------------------

using namespace std;                                     // 使用标准命名空间，避免在代码中重复写std::。

// 使用宏定义为目标点名称创建别名，但这种方式在C++中不推荐，建议使用const或enum。
#define goal1 jilin
#define goal2 shanghai
#define goal3 guangzhou
#define goal4 shengzheng
#define goal5 beijing

// ------------------------- 函数定义 (Function Definitions) -------------------------

/**
 * @brief 初始化语音文件路径数组
 * @details 将预设的介绍词语音文件的绝对路径硬编码到全局的voice数组中。
 */

// ------------------------- 全局变量定义 (Global Variables) -------------------------

string voice[10];                                        // 定义一个字符串数组，用于存储各个介绍点的语音文件路径。

// 为每个导航目标点定义全局变量，用于存储从参数服务器加载的坐标和姿态(四元数)。
// 这种定义方式较为繁琐，未来可以考虑使用结构体或类的数组来优化。
double beijing_x, shanghai_x, guangzhou_x, shenzheng_x, jilin_x, charge_x, goal6_x, goal7_x; // X坐标
double beijing_y, shanghai_y, guangzhou_y, shenzheng_y, jilin_y, charge_y, goal6_y, goal7_y; // Y坐标
double beijing_z, shanghai_z, guangzhou_z, shenzheng_z, jilin_z, charge_z, goal6_z, goal7_z; // Z方向姿态(四元数)
double beijing_w, shanghai_w, guangzhou_w, shenzheng_w, jilin_w, charge_w, goal6_w, goal7_w; // W方向姿态(四元数)

// ------------------------- 函数声明 (Function Declarations) -------------------------

void load_param();                                       // 声明一个函数，用于从ROS参数服务器加载导航点的坐标。
void voice_fun() {
    voice[1] = "/home/bobac3/bobac3_ws/src/nav_goal/voices/吉林位于我国东北是人参之都.wav";
    voice[2] = "/home/bobac3/bobac3_ws/src/nav_goal/voices/上海是中国的经济中心.wav";
    voice[3] = "/home/bobac3/bobac3_ws/src/nav_goal/voices/广州自古都是我国的商业之都.wav";
    voice[4] = "/home/bobac3/bobac3_ws/src/nav_goal/voices/深圳是中国的科创中心.wav";
    voice[5] = "/home/bobac3/bobac3_ws/src/nav_goal/voices/北京是中国的首都政治中心.wav";

}


// ------------------------- C++ 类定义 (Class Definition) -------------------------

/**
 * @class nav_goal
 * @brief 封装了与语音服务交互和导航目标发送相关的功能。
 */
class nav_goal {
public:
    // 公有成员
    nav_goal();                                         // 构造函数，用于初始化。
    string voice_collect();                             // 调用语音采集服务。
    string voice_dictation(const char* filename);       // 调用语音听写(识别)服务。
    string voice_tts(const std::string& text);          // 调用语音合成服务并播放。
    void goto_nav(struct Point* point);                 // (已声明但未定义) 导航到指定点。

private:
    // 私有成员
    ros::NodeHandle n;                                  // ROS节点句柄，是与ROS系统通信的接口。
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>* ac; // 指向move_base Action客户端的指针(已声明但未使用)。
    ros::ServiceClient collect_client, dictation_client, tts_client; // 分别定义语音采集、听写、合成的服务客户端。
};

/**
 * @brief nav_goal类的构造函数
 * @details 在对象创建时，初始化所有语音相关的服务客户端。
 */
nav_goal::nav_goal() {
    collect_client = n.serviceClient<robot_audio::Collect>("voice_collect");   // 初始化语音采集服务的客户端，连接到名为"voice_collect"的服务。
    dictation_client = n.serviceClient<robot_audio::robot_iat>("voice_iat");     // 初始化语音听写服务的客户端，连接到名为"voice_iat"的服务。
    tts_client = n.serviceClient<robot_audio::robot_tts>("voice_tts");         // 初始化语音合成服务的客户端，连接到名为"voice_tts"的服务。
}

/**
 * @brief 请求语音采集服务
 * @return string 服务端返回的录音文件路径。
 */
string nav_goal::voice_collect() {
    ros::service::waitForService("voice_collect");      // 阻塞程序，直到名为"voice_collect"的服务可用。
    robot_audio::Collect srv;                           // 创建语音采集服务的请求/响应对象。
    srv.request.collect_flag = 1;                       // 设置请求标志位为1，表示请求开始录音。
    collect_client.call(srv);                           // 发送服务请求，并等待响应。
    return srv.response.voice_filename;                 // 返回响应中包含的音频文件名。
}

/**
 * @brief 请求语音听写服务
 * @param filename 要识别的音频文件的路径。
 * @return string 服务端返回的识别出的文本。
 */
string nav_goal::voice_dictation(const char* filename) {
    ros::service::waitForService("voice_iat");          // 阻塞程序，直到名为"voice_iat"的服务可用。
    robot_audio::robot_iat srv;                         // 创建语音听写服务的请求/响应对象。
    srv.request.audiopath = filename;                   // 将要识别的音频文件路径放入请求中。
    dictation_client.call(srv);                         // 发送服务请求，并等待响应。
    return srv.response.text;                           // 返回响应中包含的识别结果文本。
}

/**
 * @brief 请求语音合成服务并播放
 * @param text 要合成的文本内容。
 * @return string 服务端返回的合成后的音频文件路径。
 */
string nav_goal::voice_tts(const std::string& text) {
    ros::service::waitForService("voice_tts");          // 阻塞程序，直到名为"voice_tts"的服务可用。
    robot_audio::robot_tts srv;                         // 创建语音合成服务的请求/响应对象。
    srv.request.text = text;                            // 将要合成的文本内容放入请求中。
    tts_client.call(srv);                               // 发送服务请求，并等待响应。
    string cmd = "play " + srv.response.audiopath;      // 构建一个shell命令，用于播放合成的音频文件。
    system(cmd.c_str());                                // 调用系统命令来执行播放操作。
    sleep(1);                                           // 等待1秒，确保有足够时间开始播放。
    return srv.response.audiopath;                      // 返回响应中包含的合成音频文件路径。
}


// ------------------------- 全局导航目标点对象定义 (Global Goal Definitions) -------------------------

// 为每个导航地点创建一个MoveBaseGoal消息对象。这些对象将用于存储发送给move_base的目标。
move_base_msgs::MoveBaseGoal beijing;
move_base_msgs::MoveBaseGoal guangzhou;
move_base_msgs::MoveBaseGoal jilin;
move_base_msgs::MoveBaseGoal shengzheng;
move_base_msgs::MoveBaseGoal shanghai;
move_base_msgs::MoveBaseGoal goal6;
move_base_msgs::MoveBaseGoal charge;
move_base_msgs::MoveBaseGoal beitai; // 虽然定义了，但在代码中并未使用
move_base_msgs::MoveBaseGoal goal7;

/**
 * @brief 设置所有导航目标点的坐标和姿态
 * @details 将从参数服务器加载到全局变量中的值，填充到对应的MoveBaseGoal消息对象中。
 */
void coordinate() {
    // 设置“北京”目标点
    beijing.target_pose.header.frame_id = "map";                  // 设置目标点的坐标系为"map"。
    beijing.target_pose.header.stamp = ros::Time::now();          // 设置时间戳为当前时间。
    beijing.target_pose.pose.position.x = beijing_x;              // 设置目标点的X坐标。
    beijing.target_pose.pose.position.y = beijing_y;              // 设置目标点的Y坐标。
    beijing.target_pose.pose.orientation.z = beijing_z;           // 设置目标姿态的Z分量(四元数)。
    beijing.target_pose.pose.orientation.w = beijing_w;           // 设置目标姿态的W分量(四元数)。

    // 设置“广州”目标点
    guangzhou.target_pose.header.frame_id = "map";                // 设置目标点的坐标系为"map"。
    guangzhou.target_pose.header.stamp = ros::Time::now();        // 设置时间戳为当前时间。
    guangzhou.target_pose.pose.position.x = guangzhou_x;          // 设置目标点的X坐标。
    guangzhou.target_pose.pose.position.y = guangzhou_y;          // 设置目标点的Y坐标。
    guangzhou.target_pose.pose.orientation.z = guangzhou_z;       // 设置目标姿态的Z分量(四元数)。
    guangzhou.target_pose.pose.orientation.w = guangzhou_w;       // 设置目标姿态的W分量(四元数)。

    // 设置“吉林”目标点
    jilin.target_pose.header.frame_id = "map";                    // 设置目标点的坐标系为"map"。
    jilin.target_pose.header.stamp = ros::Time::now();            // 设置时间戳为当前时间。
    jilin.target_pose.pose.position.x = jilin_x;                  // 设置目标点的X坐标。
    jilin.target_pose.pose.position.y = jilin_y;                  // 设置目标点的Y坐标。
    jilin.target_pose.pose.orientation.z = jilin_z;               // 设置目标姿态的Z分量(四元数)。
    jilin.target_pose.pose.orientation.w = jilin_w;               // 设置目标姿态的W分量(四元数)。

    // 设置“深圳”目标点
    shengzheng.target_pose.header.frame_id = "map";               // 设置目标点的坐标系为"map"。
    shengzheng.target_pose.header.stamp = ros::Time::now();       // 设置时间戳为当前时间。
    shengzheng.target_pose.pose.position.x = shenzheng_x;         // 设置目标点的X坐标。
    shengzheng.target_pose.pose.position.y = shenzheng_y;         // 设置目标点的Y坐标。
    shengzheng.target_pose.pose.orientation.z = shenzheng_z;      // 设置目标姿态的Z分量(四元数)。
    shengzheng.target_pose.pose.orientation.w = shenzheng_w;      // 设置目标姿态的W分量(四元数)。

    // 设置“上海”目标点
    shanghai.target_pose.header.frame_id = "map";                 // 设置目标点的坐标系为"map"。
    shanghai.target_pose.header.stamp = ros::Time::now();         // 设置时间戳为当前时间。
    shanghai.target_pose.pose.position.x = shanghai_x;            // 设置目标点的X坐标。
    shanghai.target_pose.pose.position.y = shanghai_y;            // 设置目标点的Y坐标。
    shanghai.target_pose.pose.orientation.z = shanghai_z;         // 设置目标姿态的Z分量(四元数)。
    shanghai.target_pose.pose.orientation.w = shanghai_w;         // 设置目标姿态的W分量(四元数)。

    // 设置“goal6”目标点 (可能是一个巡航结束点或中间点)
    goal6.target_pose.header.frame_id = "map";                    // 设置目标点的坐标系为"map"。
    goal6.target_pose.header.stamp = ros::Time::now();            // 设置时间戳为当前时间。
    goal6.target_pose.pose.position.x = goal6_x;                  // 设置目标点的X坐标。
    goal6.target_pose.pose.position.y = goal6_y;                  // 设置目标点的Y坐标。
    goal6.target_pose.pose.orientation.z = goal6_z;               // 设置目标姿态的Z分量(四元数)。
    goal6.target_pose.pose.orientation.w = goal6_w;               // 设置目标姿态的W分量(四元数)。
    
    // 设置“goal7”目标点 (可能是充电后的出发点或等待点)
    goal7.target_pose.header.frame_id = "map";                    // 设置目标点的坐标系为"map"。
    goal7.target_pose.header.stamp = ros::Time::now();            // 设置时间戳为当前时间。
    goal7.target_pose.pose.position.x = goal7_x;                  // 设置目标点的X坐标。
    goal7.target_pose.pose.position.y = goal7_y;                  // 设置目标点的Y坐标。
    goal7.target_pose.pose.orientation.z = goal7_z;               // 设置目标姿态的Z分量(四元数)。
    goal7.target_pose.pose.orientation.w = goal7_w;               // 设置目标姿态的W分量(四元数)。

    // 设置“charge”充电目标点
    charge.target_pose.header.frame_id = "map";                   // 设置目标点的坐标系为"map"。
    charge.target_pose.header.stamp = ros::Time::now();           // 设置时间戳为当前时间。
    charge.target_pose.pose.position.x = charge_x;                // 设置目标点的X坐标。
    charge.target_pose.pose.position.y = charge_y;                // 设置目标点的Y坐标。
    charge.target_pose.pose.orientation.z = charge_z;             // 设置目标姿态的Z分量(四元数)。
    charge.target_pose.pose.orientation.w = charge_w;             // 设置目标姿态的W分量(四元数)。
}


// ------------------------- 主函数 (Main Function) -------------------------
int main(int argc, char** argv) {
    // 初始化ROS节点，并命名为"nav_goal"。
    ros::init(argc, argv, "nav_goal");

    nav_goal audio;                                     // 创建一个nav_goal类的实例，用于处理所有语音相关的交互。
    string dir, text, path;                             // 定义字符串变量，分别用于存储音频路径、识别文本和其他路径。

    load_param();                                       // 调用函数，从ROS参数服务器加载所有预设的导航点坐标。

    // 创建一个连接到"move_base" Action服务器的客户端。
    // "true"参数表示客户端将在自己的线程中处理回调，避免阻塞主线程。
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);
    
    ROS_INFO("Waiting for action server to start.");    // 在控制台打印信息，提示正在等待Action服务器。
    ac.waitForServer();                                 // 阻塞程序，直到"move_base" Action服务器成功启动。
    ROS_INFO("Action server started, sending goal.");   // 在控制台打印信息，提示Action服务器已启动。

    coordinate();                                       // 调用函数，将加载的坐标填充到全局的导航目标对象中。
    voice_fun();                                        // 调用函数，初始化存储语音文件路径的数组。

    // 定义两个布尔型状态标志，用于控制程序的两个主要任务阶段。
    bool task1 = true;                                  // 任务1：自动巡回介绍。
    bool task2 = false;                                 // 任务2：语音点播导航。

    // 任务1：自动巡回介绍的循环。
    while (task1) {
        dir = audio.voice_collect();                    // 等待并采集用户的语音指令。
        text = audio.voice_dictation(dir.c_str()).c_str(); // 将采集到的语音进行识别。

        // 检查识别结果中是否包含唤醒词 "元宝"。
        if (text.find("元宝") != string::npos) {
            char command0[256];                         // 定义一个字符数组来存储shell命令。
            // 使用snprintf安全地格式化字符串，创建一个播放"什么事呀.wav"的命令。
            snprintf(command0, sizeof(command0), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/什么事呀.wav");
            system(command0);                           // 执行播放命令。
        }

        // 检查识别结果中是否包含关键词 "一圈"，触发巡航任务。
        if (text.find("一圈") != string::npos) {
            char command[256];                          // 定义字符数组。
            // 格式化并执行播放"这就带你参观.wav"的命令。
            snprintf(command, sizeof(command), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/这就带你参观.wav");
            system(command);
            usleep(1000 * 6);                           // 微秒级延时，等待语音播放。

            // --- 开始巡航 ---
            // 1. 前往北京
            ac.sendGoal(goal1);                         // 向move_base发送第一个目标点（北京）。
            // 循环等待，直到机器人成功到达目标点。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 循环中短暂休眠，避免CPU占用过高。
            }
            char command1[256];                         // 定义字符数组。
            // 格式化并执行播放北京介绍的语音。
            snprintf(command1, sizeof(command1), "play %s", voice[1].c_str());
            system(command1);
            usleep(1000 * 6);                           // 等待语音播放。
            
            // 2. 前往上海
            ac.sendGoal(goal2);                         // 发送第二个目标点（上海）。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 等待到达。
            }
            char command2[256];
            snprintf(command2, sizeof(command2), "play %s", voice[2].c_str());
            system(command2);                           // 播放上海介绍。
            usleep(1000 * 6);

            // 3. 前往广州
            ac.sendGoal(goal3);                         // 发送第三个目标点（广州）。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 等待到达。
            }
            char command3[256];
            snprintf(command3, sizeof(command3), "play %s", voice[3].c_str());
            system(command3);                           // 播放广州介绍。
            usleep(1000 * 6);

            // 4. 前往深圳
            ac.sendGoal(goal4);                         // 发送第四个目标点（深圳）。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 等待到达。
            }
            char command4[256];
            snprintf(command4, sizeof(command4), "play %s", voice[4].c_str());
            system(command4);                           // 播放深圳介绍。
            usleep(1000 * 6);

            // 5. 前往吉林
            ac.sendGoal(goal5);                         // 发送第五个目标点（吉林）。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 等待到达。
            }
            char command5[256];
            snprintf(command5, sizeof(command5), "play %s", voice[5].c_str());
            system(command5);                           // 播放吉林介绍。
            usleep(1000 * 6);

            // 6. 前往最终点
            ac.sendGoal(goal6);                         // 发送最后一个目标点（巡航结束点）。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 等待到达。
            }
            
            task1 = false;                              // 巡航任务完成，将task1标志设为false，退出当前循环。
        }
        task2 = true;                                   // 准备进入task2状态。
    }

    // 任务2：语音点播导航的循环。
    while (task2) {
        dir = audio.voice_collect();                    // 采集语音。
        text = audio.voice_dictation(dir.c_str()).c_str(); // 识别语音。
        
        // 同样检查唤醒词。
        if (text.find("元宝") != string::npos) {
            char command0[256];
            snprintf(command0, sizeof(command0), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/什么事呀.wav");
            system(command0);
        }

        // 检查是否要去“北京”。
        if (text.find("北京") != string::npos) {
            char command1[256];
            snprintf(command1, sizeof(command1), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/去北京馆.wav");
            system(command1);                           // 播放提示音“去北京馆”。
            usleep(1000 * 1);

            ac.sendGoal(beijing);                       // 发送北京目标点。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);                      // 等待到达。
            }

            char command2[256];
            snprintf(command2, sizeof(command2), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/北京是中国的首都政治中心.wav");
            system(command2);                           // 到达后播放介绍。
            usleep(1000 * 1);
            
            // --- 自动回充逻辑 ---
            ac.sendGoal(charge);                        // 导航到充电桩附近。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);                      // 等待到达。
            }
            char command[256];
            // 通过系统命令启动二次定位/精准对接充电桩的launch文件，'&'使其在后台运行。
            snprintf(command, sizeof(command), "roslaunch %s %s", "secondary_localization", "track.launch &");
            system(command);
            usleep(1000 * 12000);                       // 等待12秒，给二次定位留出足够时间。
            
            // 通过系统命令杀死二次定位相关的节点，清理环境。
            system("rosnode kill /relative_move");

            char command3[256];
            snprintf(command3, sizeof(command3), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/充电成功.wav");
            system(command3);                           // 播放充电成功提示音。
            usleep(1000 * 6);

            ac.sendGoal(goal7);                         // 导航到充电后的等待点。
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);                      // 等待到达。
            }
        }
        // 后续的else if块逻辑与"北京"部分完全相同，只是目标点和语音内容不同。
        else if (text.find("广州") != string::npos) {
            // ... (导航到广州 -> 播放介绍 -> 回充 -> 前往等待点)
            char command1[256];
            snprintf(command1, sizeof(command1), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/去广州馆.wav");
            system(command1);
            usleep(1000 * 1);

            ac.sendGoal(guangzhou);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }

            char command2[256];
            snprintf(command2, sizeof(command2), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/广州自古都是我国的商业之都.wav");
            system(command2);
            usleep(1000 * 1);
            
            ac.sendGoal(charge);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command[256];
            snprintf(command, sizeof(command), "roslaunch %s %s", "secondary_localization", "track.launch &");
            system(command);
            usleep(1000 * 12000);
            system("rosnode kill /relative_move");

            char command3[256];
            snprintf(command3, sizeof(command3), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/充电成功.wav"); // 注意：这里有个拼写错误oices
            system(command3);
            usleep(1000 * 6);

            ac.sendGoal(goal7);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);
            }
        }
        else if (text.find("吉林") != string::npos) {
            // ... (导航到吉林 -> 播放介绍 -> 回充 -> 前往等待点)
            char command1[256];
            snprintf(command1, sizeof(command1), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/去吉林馆.wav");
            system(command1);
            usleep(1000 * 1);

            ac.sendGoal(jilin);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command2[256];
            snprintf(command2, sizeof(command2), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/吉林位于我国东北是人参之都.wav");
            system(command2);
            usleep(1000 * 1);
            
            ac.sendGoal(charge);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command[256];
            snprintf(command, sizeof(command), "roslaunch %s %s", "secondary_localization", "track.launch &");
            system(command);
            usleep(1000 * 12000);
            system("rosnode kill /relative_move");

            char command3[256];
            snprintf(command3, sizeof(command3), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/充电成功.wav");
            system(command3);
            usleep(1000 * 6);

            ac.sendGoal(goal7);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);
            }
        }
        else if (text.find("深圳") != string::npos) {
            // ... (导航到深圳 -> 播放介绍 -> 回充 -> 前往等待点)
            char command1[256];
            snprintf(command1, sizeof(command1), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/去深圳馆.wav");
            system(command1);
            usleep(1000 * 1);

            ac.sendGoal(shengzheng);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command2[256];
            snprintf(command2, sizeof(command2), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/深圳是中国的科创中心.wav");
            system(command2);
            usleep(1000 * 1);

            ac.sendGoal(charge);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command[256];
            snprintf(command, sizeof(command), "roslaunch %s %s", "secondary_localization", "track.launch &");
            system(command);
            usleep(1000 * 12000);
            system("rosnode kill /relative_move");

            char command3[256];
            snprintf(command3, sizeof(command3), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/充电成功.wav");
            system(command3);
            usleep(1000 * 6);

            ac.sendGoal(goal7);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);
            }
        }
        else if (text.find("上海") != string::npos) {
            // ... (导航到上海 -> 播放介绍 -> 回充 -> 前往等待点)
            char command1[256];
            snprintf(command1, sizeof(command1), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/去上海馆.wav");
            system(command1);
            usleep(1000 * 1);

            ac.sendGoal(shanghai);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command2[256];
            snprintf(command2, sizeof(command2), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/上海是中国的经济中心.wav");
            system(command2);
            usleep(1000 * 1);
            
            ac.sendGoal(charge);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 10);
            }
            char command[256];
            snprintf(command, sizeof(command), "roslaunch %s %s", "secondary_localization", "track.launch &");
            system(command);
            usleep(1000 * 12000);
            system("rosnode kill /relative_move");

            char command3[256];
            snprintf(command3, sizeof(command3), "play %s", "/home/bobac3/bobac3_ws/src/nav_goal/voices/充电成功.wav");
            system(command3);
            usleep(1000 * 6);

            ac.sendGoal(goal7);
            while (!(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)) {
                usleep(1000 * 20);
            }
        }
    }

    return 0; // 程序正常结束，返回0。
}


/**
 * @brief 从ROS参数服务器加载导航点的坐标
 * @details 使用ros::param::get()函数，根据参数名获取对应的坐标值，并存入全局变量。
 * 这些参数通常在.launch文件或.yaml文件中定义。
 */
void load_param() {
    // 加载北京的坐标参数
    ros::param::get("/beijing_x", beijing_x);
    ros::param::get("/beijing_y", beijing_y);
    ros::param::get("/beijing_z", beijing_z);
    ros::param::get("/beijing_w", beijing_w);

    // 加载上海的坐标参数
    ros::param::get("/shanghai_x", shanghai_x);
    ros::param::get("/shanghai_y", shanghai_y);
    ros::param::get("/shanghai_z", shanghai_z);
    ros::param::get("/shanghai_w", shanghai_w);

    // 加载广州的坐标参数
    ros::param::get("/guangzhou_x", guangzhou_x);
    ros::param::get("/guangzhou_y", guangzhou_y);
    ros::param::get("/guangzhou_z", guangzhou_z);
    ros::param::get("/guangzhou_w", guangzhou_w);

    // 加载深圳的坐标参数 (注意参数名是shenzhen，变量名是shengzheng)
    ros::param::get("/shenzhen_x", shenzheng_x);
    ros::param::get("/shenzhen_y", shenzheng_y);
    ros::param::get("/shenzhen_z", shenzheng_z);
    ros::param::get("/shenzhen_w", shenzheng_w);

    // 加载吉林的坐标参数
    ros::param::get("/jilin_x", jilin_x);
    ros::param::get("/jilin_y", jilin_y);
    ros::param::get("/jilin_z", jilin_z);
    ros::param::get("/jilin_w", jilin_w);

    // 加载充电点的坐标参数
    ros::param::get("/charge_x", charge_x);
    ros::param::get("/charge_y", charge_y);
    ros::param::get("/charge_z", charge_z);
    ros::param::get("/charge_w", charge_w);

    // 加载goal6的坐标参数
    ros::param::get("/goal6_x", goal6_x);
    ros::param::get("/goal6_y", goal6_y);
    ros::param::get("/goal6_z", goal6_z);
    ros::param::get("/goal6_w", goal6_w);

    // 加载goal7的坐标参数
    ros::param::get("/goal7_x", goal7_x);
    ros::param::get("/goal7_y", goal7_y);
    ros::param::get("/goal7_z", goal7_z);
    ros::param::get("/goal7_w", goal7_w);
}
